//
// Created by jiang on 2021/12/15.
//

#include <stdlib.h>
#include "current.h"
#include "pwm.h"
#include "adcollect.h"
#include "filter.h"

curr_t current;

const int16_t NUM[IIR_NL] = {
        335,   1338,   2007,   1338,    335
};
const int16_t DEN[IIR_NL] = {
        32767, -32768,  32767, -23739,   3945
};

//const float NUM[IIR_NL] = {
//        0.02995458245f,  0.05990916491f,  0.02995458245f
//};
//const float DEN[IIR_NL] = {
//        1,   -1.454243541f,   0.5740619302f
//};

void CURR_Init(curr_t *pHandle) {

    ADC_Init(&adc);

    /* 3300000  : ADC reference voltage(uv)
     * 4096     : ADC precision
     * 0.1f     : Visual resistance base the real resistance and amplifier */
    pHandle->gain = 3300000 * 10 / 4096;


#if defined(USE_CURR_AB) /**************************/
    pHandle->phase.a = 0;
    pHandle->phase.b = 0;
    pHandle->calib.a = 0;
    pHandle->calib.b = 0;
    pHandle->filter.a = 0;
    pHandle->filter.b = 0;

#if defined(USE_CURR_FILTER_LPF)
    LPF_Init(&pHandle->lpfa, 500);
    LPF_Init(&pHandle->lpfb, 500);

#elif defined(USE_CURR_FILTER_KALMAN)
    Kalman_Init(&pHandle->klma, 250, 3900);
    Kalman_Init(&pHandle->klmb, 250, 500);

#elif defined(USE_CURR_FILTER_IIR)
    IIR_Init(&pHandle->irra, NUM, DEN);
    IIR_Init(&pHandle->irrb, NUM, DEN);

#endif

#elif defined(USE_CURR_BC) /**************************/
    pHandle->phase.b = 0;
    pHandle->phase.c = 0;
    pHandle->calib.b = 0;
    pHandle->calib.c = 0;
    pHandle->filter.b = 0;
    pHandle->filter.c = 0;

#if defined(USE_CURR_FILTER_LPF)
    LPF_Init(&pHandle->lpfb, 750);
    LPF_Init(&pHandle->lpfc, 750);

#elif defined(USE_CURR_FILTER_KALMAN)
    Kalman_Init(&pHandle->klmb, 250, 3900);
    Kalman_Init(&pHandle->klmc, 250, 500);

#elif defined(USE_CURR_FILTER_IIR)
    IIR_Init(&pHandle->irrb, NUM, DEN);
    IIR_Init(&pHandle->irrc, NUM, DEN);

#endif

#elif defined(USE_CURR_AC) /**************************/
    pHandle->phase.a = 0;
    pHandle->phase.c = 0;
    pHandle->calib.a = 0;
    pHandle->calib.c = 0;
    pHandle->filter.a = 0;
    pHandle->filter.c = 0;

#if defined(USE_CURR_FILTER_LPF)
    LPF_Init(&pHandle->lpfa, 500);
    LPF_Init(&pHandle->lpfc, 500);

#elif defined(USE_CURR_FILTER_KALMAN)
    Kalman_Init(&pHandle->klma, 250, 3900);
    Kalman_Init(&pHandle->klmc, 250, 500);

#elif defined(USE_CURR_FILTER_IIR)
    IIR_Init(&pHandle->irra);
    IIR_Init(&pHandle->irrc);

#endif
#endif

}

static void CURR_GetOffset(curr_t *pHandle, adc_t *aHandle) {
    uint32_t sum[2] = {0};

    for (uint8_t i = 0; i < 10; i++) {
        sum[0] += aHandle->adcValue[2];
        sum[1] += aHandle->adcValue[3];
        HAL_Delay(20);
    }
    sum[0] /= 10;
    sum[1] /= 10;

#if defined(USE_CURR_AB)
    pHandle->calib.a = (((uint16_t)sum[0] - 2048) * pHandle->gain) / 1000;
    pHandle->calib.b = (((uint16_t)sum[1] - 2048) * pHandle->gain) / 1000;

    if (abs(pHandle->calib.a) > 5000 || abs(pHandle->calib.b) > 5000)
        ERR_State.bits.ADC_error = 1;

#elif defined(USE_CURR_BC)
    pHandle->calib.b = (((uint16_t) sum[0] - 2048) * pHandle->gain) / 1000;
    pHandle->calib.c = (((uint16_t) sum[1] - 2048) * pHandle->gain) / 1000;

    if (abs(pHandle->calib.b) > 5000 || abs(pHandle->calib.c) > 5000)
        ERR_State.bits.ADC_error = 1;

#elif defined(USE_CURR_AC)
    pHandle->calib.a = (((uint16_t)sum[0] - 2048) * pHandle->gain) / 1000;
    pHandle->calib.c = (((uint16_t)sum[1] - 2048) * pHandle->gain) / 1000;

    if (abs(pHandle->calib.a) > 5000 || abs(pHandle->calib.c) > 5000)
        ERR_State.bits.ADC_error = 1;

#endif
}


inline void CURR_Calibration(curr_t *pHandle) {
    ADC_Start(&adc);

    PWM_EnableTrigger(&pwm);

    HAL_Delay(200);

    CURR_GetOffset(&current, &adc);
}


void CURR_GetFromAdcVal(curr_t *pHandle, adc_t *aHandle) {
#if defined(USE_CURR_AB) /**************************/
    pHandle->phase.a = ((aHandle->adcValue[2] - 2048) * pHandle->gain) / 1000 - pHandle->calib.a;
    pHandle->phase.b = ((aHandle->adcValue[3] - 2048) * pHandle->gain) / 1000 - pHandle->calib.b;

#if defined(USE_CURR_FILTER_LPF)
    pHandle->filter.a = LPF_Filter(&pHandle->lpfa, pHandle->phase.a, pHandle->filter.a);
    pHandle->filter.b = LPF_Filter(&pHandle->lpfb, pHandle->phase.b, pHandle->filter.b);

#elif defined(USE_CURR_FILTER_KALMAN)
    pHandle->filter.a = Kalman_Filter(&pHandle->klma, pHandle->phase.a, pHandle->filter.a);
    pHandle->filter.b = Kalman_Filter(&pHandle->klmb, pHandle->phase.b, pHandle->filter.b);

#elif defined(USE_CURR_FILTER_IIR)
    pHandle->filter.a = IIR_Filter(&pHandle->irra, pHandle->phase.a);
    pHandle->filter.b = IIR_Filter(&pHandle->irrb, pHandle->phase.b);

#endif

#elif defined(USE_CURR_BC) /**************************/
    pHandle->phase.b = ((aHandle->adcValue[2] - 2048) * pHandle->gain) / 1000 - pHandle->calib.b;
    pHandle->phase.c = ((aHandle->adcValue[3] - 2048) * pHandle->gain) / 1000 - pHandle->calib.c;

#if defined(USE_CURR_FILTER_LPF)
    pHandle->filter.b = LPF_Filter(&pHandle->lpfb, pHandle->phase.b, pHandle->filter.b);
    pHandle->filter.c = LPF_Filter(&pHandle->lpfc, pHandle->phase.c, pHandle->filter.c);

#elif defined(USE_CURR_FILTER_KALMAN)
    pHandle->filter.b = Kalman_Filter(&pHandle->klmb, pHandle->phase.b, pHandle->filter.b);
    pHandle->filter.c = Kalman_Filter(&pHandle->klmc, pHandle->phase.c, pHandle->filter.c);

#elif defined(USE_CURR_FILTER_IIR)
    pHandle->filter.b = IIR_Filter(&pHandle->irrb, pHandle->phase.b);
    pHandle->filter.c = IIR_Filter(&pHandle->irrc, pHandle->phase.c);

#endif

#elif defined(USE_CURR_AC) /**************************/
    pHandle->phase.a = ((aHandle->adcValue[2] - 2048) * pHandle->gain) / 1000 - pHandle->calib.a;
    pHandle->phase.c = ((aHandle->adcValue[3] - 2048) * pHandle->gain) / 1000 - pHandle->calib.c;

#if defined(USE_CURR_FILTER_LPF)
    pHandle->filter.a = LPF_Filter(&pHandle->lpfa, pHandle->phase.a, pHandle->filter.a);
    pHandle->filter.c = LPF_Filter(&pHandle->lpfc, pHandle->phase.c, pHandle->filter.c);

#elif defined(USE_CURR_FILTER_KALMAN)
    pHandle->filter.a = Kalman_Filter(&pHandle->klma, pHandle->phase.a, pHandle->filter.a);
    pHandle->filter.c = Kalman_Filter(&pHandle->klmc, pHandle->phase.c, pHandle->filter.c);

#elif defined(USE_CURR_FILTER_IIR)
    pHandle->filter.a = IIR_Filter(&pHandle->irra, pHandle->phase.a);
    pHandle->filter.c = IIR_Filter(&pHandle->irrc, pHandle->phase.c);

#endif

#endif


}





